#!/usr/bin/env python
#Nz9fZ2VT4Sh8

import roslib; roslib.load_manifest('people_recognition')
import rospy, sys, math, time, colorsys
import message_filters, sensor_msgs, actionlib, tf
import image_geometry
from storage.msg import *
from people_recognition.msg import *
from geometry_msgs.msg import PoseStamped, Point, Pose, PointStamped
from sensor_msgs.msg import PointCloud, Image, JointState, CameraInfo



class PeopleRecognitionServer:

	def __init__(self):
		rospy.logwarn('Server Init...')
		self.server = actionlib.SimpleActionServer('recognition_server', PersonAction, self.execute, False)
		self.server.start()		
		self.face_detect_subscriber = message_filters.Subscriber('/face_detector/faces_cloud', PointCloud)
		self.camera_subscriber = message_filters.Subscriber('/wide_stereo/left/image_rect_color', Image)
		self.right_camera_info = message_filters.Subscriber('/wide_stereo/right/camera_info', CameraInfo)
		self.left_camera_info = message_filters.Subscriber('/wide_stereo/left/camera_info', CameraInfo)
		self.tl = tf.TransformListener()
		self.strCamConv = image_geometry.StereoCameraModel()
		self.database = actionlib.SimpleActionClient("addPerson",addPersonAction)
		self.database.wait_for_server()
		self.once = False
		subscribers = [self.face_detect_subscriber, self.camera_subscriber, self.left_camera_info, self.right_camera_info]
		self.time_synch = message_filters.TimeSynchronizer(subscribers, 15)
		self.time_synch.registerCallback(self.recognize)
		
		self.averageDB = []
		self.creatingAverageDB()
		rospy.logwarn(self.averageDB)
		self.imageNum = 0
		rospy.logwarn('Server Started\n')

	def creatingAverageDB(self):
		rospy.logwarn('Creating averageDB')
		goal=addPersonGoal()
		goal.target.append("get")
		goal.target.append("mainBio")
		self.database.send_goal(goal)
		self.database.wait_for_result()
		res = self.database.get_result()
		for i in range(len(res.result)):
			values = res.result[i].split(",")
			values[9]=values[9].strip(')')
			self.imageNum = float(values[1])
			person_id = float(values[2])
			height = float(values[3])
			face_r = float(values[4])
			face_g = float(values[5])
			face_b = float(values[6])
			shirt_r = float(values[7])	
			shirt_g = float(values[8])
			shirt_b = float(values[9])
			rowNum =-1
			for j in range(len(self.averageDB)):
				if self.averageDB[j][0] == person_id:
					rowNum = j
					break
			if(rowNum==-1):
				personRow = [person_id, height, face_r,face_g, face_b, shirt_r, shirt_g, shirt_b, 1]
				self.averageDB.append(personRow)
			else:
				rowToUpdate = self.averageDB[rowNum]
				count = rowToUpdate[8]
				for k in range (7):
					rowToUpdate[k+1] =(rowToUpdate[k+1]*count+float(values[k+3]))/(count+1)
				rowToUpdate[8]+= 1
				self.averageDB[rowNum]= rowToUpdate				

	def execute(self, goal):
		rospy.logwarn('Server Execute...')
		self.once =True
		self.imageNum += 1

		self.result = PersonResult()
		self.result.people = []

		while(self.once):
			time.sleep(.1)

		rospy.logwarn('Server Success\n')
		self.server.set_succeeded(result = self.result)

	def recognize(self, face_cloud, image, left_info, right_info):
		if self.once is True:
			rospy.logwarn('Server Recognize...')
			self.strCamConv.fromCameraInfo(left_info, right_info)
			self.seenFaces=[]
			try:
				cloud = self.tl.transformPointCloud('base_footprint', face_cloud)
				for index in range(0, len(cloud.points)):
					###setup points and point clouds###
					
					raw_point = face_cloud.points[index]
					transformed_point = cloud.points[index]
					raw_point3d = [raw_point.x, raw_point.y, raw_point.z]
					pixelArray = self.strCamConv.project3dToPixel(raw_point3d)

					###get height###
					person = Person_Identifier()
					person.x = transformed_point.x
					person.y = transformed_point.y
					person.z = transformed_point.z

					###get face color###
					face = self.get_face(round(pixelArray[0][0]), round(pixelArray[0][1]), raw_point.z, image)
					person.face_red = face[0]
					person.face_green = face[1]
					person.face_blue = face[2]

					###get shirt color###
					shirt = self.get_shirt(round(pixelArray[0][0]), round(pixelArray[0][1]+50), raw_point.z, image)
					person.shirt_red = shirt[0]
					person.shirt_green = shirt[1]
					person.shirt_blue = shirt[2]
					
					###add person to result array###
					person.identifier = self.get_identifier(person.z, face, shirt)
					self.seenFaces.append(int(person.identifier))
		
					
					goal=addPersonGoal()
					#get or set
					goal.target.append("set")
					#table
					goal.target.append("mainBio")
					#imageID
					goal.target.append(str(self.imageNum))
					#personID
					goal.target.append(str(person.identifier))
					#height
					goal.target.append(str(person.z))
					#faceR
					goal.target.append(str(face[0]))
					#faceG
					goal.target.append(str(face[1]))
					#faceB
					goal.target.append(str(face[2]))
					#shirtR
					goal.target.append(str(shirt[0]))
					#shirtG
					goal.target.append(str(shirt[1]))
					#shirtB
					goal.target.append(str(shirt[2]))

					self.database.send_goal(goal)
					self.database.wait_for_result()
					self.result.people.append(person)

					goal=addPersonGoal()
					goal.target.append("get")
					goal.target.append("mainBio")
					self.database.send_goal(goal)
					self.database.wait_for_result()
					res = self.database.get_result()
					for i in range(len(res.result)):
						rospy.logwarn(res.result[i])
	
			except (tf.LookupException):
				rospy.logwarn('Lookup exception...')
			self.once = False

		
	def get_face(self, x, y, z, image):
		center = int((round(x)+ image.width*round(y))*3)
		blue = 0
		green = 0
		red = 0
		counter = 0

		y = int(y)
		x = int(x)
		rospy.logwarn(str(x) + ", " + str(y))
		for x_coord in range(x-6, x+7):
			for y_coord in range (y, y+9):
				if (x_coord < 640 and y_coord < 480 and x_coord >= 0 and y_coord >= 0):
					pixel = int((x_coord+ image.width*y_coord)*3)
					blue = blue + ord(image.data[pixel])
					green = green + ord(image.data[pixel+1])
					red = red + ord(image.data[pixel+2])
					counter = counter + 1
		blue = blue/counter
		green = green/counter
		red = red/counter
		return [red, green, blue]

	def get_shirt(self, x, y, z, image):
		center = int((round(x)+ image.width*round(y))*3)
		blue = 0
		green = 0
		red = 0
		counter = 0

		x = int(x)
		y = int(y)
		for x_coord in range(x-14, x+15):
			for y_coord in range (y-15, y):
				if (x_coord < 640 and y_coord < 480 and x_coord >= 0 and y_coord >= 0):
					pixel = int((x_coord+ image.width*y_coord)*3)
					blue = blue + ord(image.data[pixel])
					green = green + ord(image.data[pixel+1])
					red = red + ord(image.data[pixel+2])
					counter = counter + 1
		blue = blue/counter
		green = green/counter
		red = red/counter
		
		return [red, green, blue]

	def get_identifier(self, height, face, shirt):
		# Finds both possible bound matches and ratio matches. If there are no bound matches, check the ratio matches. 
		# We have thresholds of 30% between the highest ratio difference and smallest ratio difference for a ratio match
		# we also have a threshold of 10 for the rgb components in the bounds case.
		# It is possible to modify the bounds threshold by changing rgbFT (face) and rgbST (shirt)
		# Possible to change ratio threshold by changing ratioThreshold
		# If you have multiple matches, it does a sum of squared differences on the rgb components and returns the min
		# We did not include height in the sum of squared differences
		possibleMatches =[]
		possibleRMatches = []
		for i in range(len(self.averageDB)):
			if ((i+1) in self.seenFaces):
				continue
			avgPerInfo =self.averageDB[i]
			if abs(height-avgPerInfo[1])<.05:
				rgbFT=10
				rgbST=10
				ratioThreshold=.3
				faceSimilar = (abs(face[0]-avgPerInfo[2])<rgbFT and abs(face[1]-avgPerInfo[3])<rgbFT and abs(face[2]-avgPerInfo[4])<rgbFT)
				shirtSimilar = (abs(shirt[0]-avgPerInfo[5])<rgbST and abs(shirt[1]-avgPerInfo[6])<rgbST and abs(shirt[2]-avgPerInfo[7])<rgbST)
				if (faceSimilar and shirtSimilar):
					possibleMatches.append(i)
				else:
					fRatioR = (face[0]-avgPerInfo[2])/avgPerInfo[2]
					fRatioG = (face[1]-avgPerInfo[3])/avgPerInfo[3]
					fRatioB = (face[2]-avgPerInfo[4])/avgPerInfo[4]
					sRatioR = (shirt[0]-avgPerInfo[5])/avgPerInfo[5]
					sRatioG = (shirt[1]-avgPerInfo[6])/avgPerInfo[6]
					sRatioB = (shirt[2]-avgPerInfo[7])/avgPerInfo[7]
					if fRatioR>=0 and fRatioG>=0 and fRatioB>=0 and sRatioR>=0 and sRatioG>=0 and sRatioB>=0:
						smallestRatio = min(fRatioR, fRatioG, fRatioB, sRatioR, sRatioB, sRatioG)
						largestRatio = max(fRatioR, fRatioG, fRatioB, sRatioR, sRatioB, sRatioG)
						if(largestRatio-smallestRatio)<ratioThreshold:
							possibleRMatches.append(i)
					elif fRatioR<=0 and fRatioG<=0 and fRatioB<=0 and sRatioR<=0 and sRatioG<=0 and sRatioB<=0:
						smallestRatio = min(fRatioR, fRatioG, fRatioB, sRatioR, sRatioB, sRatioG)
						largestRatio = max(fRatioR, fRatioG, fRatioB, sRatioR, sRatioB, sRatioG)
						if(largestRatio-smallestRatio)<ratioThreshold:
							possibleRMatches.append(i)

		
		combinedRGB = face
		combinedRGB.extend(shirt)
		closestMatch = (len(self.averageDB)+1)
		if len(possibleMatches)>0:
			rospy.logwarn("bounds case")
			minSSD=sys.maxint
			for i in range(len(possibleMatches)):
				aMatch = self.averageDB[possibleMatches[i]]
				SSD = 0
				for j in range(6):
					SSD = SSD+math.pow(combinedRGB[j]-aMatch[j+2],2)
				SSD = math.sqrt(SSD)
				if(SSD<minSSD):
					minSSD = SSD
					closestMatch= possibleMatches[i]+1
		elif len(possibleRMatches)>0:
			rospy.logwarn("ratio case")
			minSSD=sys.maxint
			for i in range(len(possibleRMatches)):
				aMatch = self.averageDB[possibleRMatches[i]]
				SSD = 0
				for j in range(6):
					SSD = SSD+math.pow(combinedRGB[j]-aMatch[j+2],2)
				SSD = math.sqrt(SSD)
				if(SSD<minSSD):
					minSSD = SSD
					closestMatch= possibleRMatches[i]+1
		if closestMatch<=len(self.averageDB):
			rospy.logwarn("there is a match")
			rowNum = closestMatch-1

			values = [height]
			values.extend(combinedRGB)
			rowToUpdate = self.averageDB[rowNum]
			count = rowToUpdate[8]
			for k in range (7):
				rowToUpdate[k+1] =(rowToUpdate[k+1]*count+values[k])/(count+1)
			rowToUpdate[8]+= 1
			self.averageDB[rowNum]= rowToUpdate
		else:
			rospy.logwarn("no match")
			identifier = len(self.averageDB)+1
			values = [identifier, height]
			values.extend(combinedRGB)
			values.append(1)
			self.averageDB.append(values)
		rospy.logwarn(self.averageDB)
		return closestMatch
			
if __name__ == '__main__':
	rospy.logwarn('Starting Server...')
	rospy.init_node('people_recognition_server')
	server = PeopleRecognitionServer()
	rospy.spin()
